#!/usr/bin/env pybricks-micropython

from pybricks.hubs import EV3Brick
from pybricks.ev3devices import Motor
from pybricks.parameters import Port
from pybricks.robotics import DriveBase
from pybricks.parameters import Color, Stop,Direction
from pybricks.ev3devices import TouchSensor,ColorSensor,UltrasonicSensor,InfraredSensor

class MyRobot:
 
    def __init__(self):
        print("MyRobot starting  ------------")
        # Initialize the motors.
        self.ev3 =EV3Brick()
        self.left_motor      = Motor(Port.A)
        self.right_motor     = Motor(Port.B)
        self.zhuaGan_motor   = Motor(Port.C)
        self.zhuaZi_motor    = Motor(Port.D)
        self.switch      = TouchSensor(Port.S1)
        self.colorChecker= ColorSensor(Port.S2) 
        self.ultrasonic  = UltrasonicSensor(Port.S3)
        self.infrared    = InfraredSensor(Port.S4)
        
        self.ev3.speaker.set_speech_options('zh','f1',100,100)
        # Initialize the drive base.
        self.robot = DriveBase(self.left_motor, self.right_motor, wheel_diameter=10.8, axle_track=205)  
        self.robot.settings(35,60,25,60)
        # Initialize variable
        self.ZhuaGuan_DEGREE_MAX  =  17540
        self.ZhuaGuan_DEGREE_CURR =  17540
        self.zhuaZi_DEGREE_MAX    =  17540
        self.zhuaZi_DEGREE_CURR   =  17540

    def paySound(self,sound):
        self.ev3.speaker.play_notes(sound)

    def goAhead(self, distance=0):
        print("-- goAhead  -- " + str(distance) + " cm")
        self.robot.straight(distance)
       
    def goDrive(self,speed=25):
        print("goDrive  ------------")
        self.robot.drive(speed,0)

    def goDriveStop(self):
        print("goDriveStop  ------------")
        self.robot.stop()     


    def turn(self, degree):
        # print("-- turn     -- "+ str(degree) +" o")
        self.robot.turn(degree)
        

    def zhuaGanUp(self,angle=180):
        # print("-- zhua Gan Up   --"+ str(angle)+" o")
        # print(self.switch.pressed())
        if not(self.switch.pressed()):
            self.zhuaGan_motor.run_angle(360,angle,Stop.HOLD,True)  
            self.ZhuaGuan_DEGREE_CURR = self.ZhuaGuan_DEGREE_CURR + 180
            self.zhuaGan_motor.reset_angle(0)
        else:
            print("It is max up. can not up")

    def zhuaGanUpByCheck(self,angle=180):
        firstUp= 1800
        self.zhuaGan_motor.run_angle(560,firstUp,Stop.HOLD,True)  
        print("-- zhua Gan Up   --"+ str(angle)+" o")
        num = (angle-firstUp)/180
        i=0
        while i<num:
            self.zhuaGanUp(180)
            i=i+1

    def zhuaGanDown(self,angle=180):
        print("-- zhua Gan Down  --"+ str(angle)+" o")
        if (self.ZhuaGuan_DEGREE_CURR >0):
           self.zhuaGan_motor.run_angle(-540,angle,Stop.HOLD,True)  
           self.ZhuaGuan_DEGREE_CURR = self.ZhuaGuan_DEGREE_CURR - 180
           print(self.ZhuaGuan_DEGREE_CURR)
           print("zhuagaun"+str(self.zhuaGan_motor.angle()))
        else:
           print("It is min down. can not down")

    def zhuaZiInitalPostion(self):
        self.zhuaZi_motor.reset_angle(0)
        self.zhuaZi_DEGREE_CURR   =  0

    def zhuaZiOpen(self,degree):
        print("zhua zi Open  ------------")
        self.zhuaZi_motor.run_angle(360,degree,Stop.HOLD,True)  
        self.zhuaZi_DEGREE_CURR= self.zhuaZi_DEGREE_CURR + degree

    def zhuaZiClose(self,degree):
        print("zhua Zi Close  ------------")
        if (self.zhuaZi_DEGREE_CURR >0):
           self.zhuaZi_motor.run_angle(-360,degree,Stop.HOLD,True)  
           self.zhuaZi = self.zhuaZi_DEGREE_CURR - degree
           print(self.zhuaZi_DEGREE_CURR)
           print("zhuaZi"+str(self.zhuaZi_motor.angle()))
        else:
           print("It is min down. can not down")

    def checkDisLimist(self, degree):
        MAX_DIS = 500
        MIN_DIS = 200
        if degree>MAX_DIS:
            return MAX_DIS
        if degree<MIN_DIS:
            return MIN_DIS       
        return degree 

    def checkObjectByDegree(self,degree):
        print("check Object ------------")
        MAX_DIS = 500
        MIN_DIS = 200

        currDistance = self.ultrasonic.distance()
        self.turn(degree)
        newDistance = self.ultrasonic.distance()
        print('Degree:'+str(degree)+' CurrDis:'+str(currDistance)+' NewDis:'+str(newDistance))
        if (self.checkDisLimist(currDistance) <= self.checkDisLimist(newDistance)):
            print("Cup is in current")
            return True
        else:
            print("Cup is in newDistance")
            return False
    
    def checkObjectByLargeDegree(self,startDegree,endDegree,stepDegree,offset=0):
        print("checkObjectByLargeDegree---start-1--")
        resList=[]
        num = (endDegree-startDegree) / stepDegree
        self.turn(startDegree)
        resList.append(self.ultrasonic.distance())
        i=1
        while i < num :
        #    print("check Distance "+ str(i) +": startDis:"+ str(startDegree+ (i-1)*stepDegree) + " endDis:"+ str(startDegree+(i*stepDegree)))
           self.turn(stepDegree )
           resList.append(self.ultrasonic.distance())
           i=i+1
        lastDeg=(endDegree - (startDegree+ (i-1)*stepDegree))
        # print("check Distance "+ str(i) +": startDis:"+ str(startDegree+ (i-1)*stepDegree) + " endDis:"+ str(startDegree+ (i-1)*stepDegree+lastDeg))
        self.turn(lastDeg)
        resList.append(self.ultrasonic.distance())
 
        minIndex = resList.index(min(resList))
        # print("checkObjectByLargeDegree---start-2--")
        print("minIndex is:" + str(minIndex))
        print(resList)
        reCheck=[]
        ind=1
        resIndex=minIndex
        while ind< len(resList):
            if (resList[ind]==resList[minIndex]):
                reCheck.append(ind)
            ind=ind+1
        print("reCheck:")
        print(reCheck)
        if len(reCheck)>1:
              temp=int(len(reCheck)/2)
              resIndex = temp
        print("resIndex "+ str(resIndex))   
        if(resIndex < i):
            lastTurnDeg=(lastDeg+ (i-resIndex-1)*stepDegree - offset)*-1 
            print("turn to the resIndex degree "+ str(lastTurnDeg))
            self.turn(lastTurnDeg)
        else:
            print("No need turn since current is resIndex")
     
        return resList[resIndex]


    def checkColorByLargeDegree(self,checkColor, startDegree,endDegree,stepDegree,offset=0):
        print("checkColorByLargeDegree---start-1--")
        #1.根据角度计算转向的次数
        resList=[]
        num = (endDegree-startDegree) / stepDegree
        self.turn(startDegree)
        resList.append(self.colorChecker.color())
        i=1
        #2.依次侧旋转，测量颜色
        while i < num :
        #    print("check color "+ str(i) +": startDis:"+ str(startDegree+ (i-1)*stepDegree) + " endDis:"+ str(startDegree+(i*stepDegree)))
           self.turn(stepDegree )
           resList.append(self.colorChecker.color())
           i=i+1
        lastDeg=(endDegree - (startDegree+ (i-1)*stepDegree))
        # print("check color "+ str(i) +": startDis:"+ str(startDegree+ (i-1)*stepDegree) + " endDis:"+ str(startDegree+ (i-1)*stepDegree+lastDeg))
        self.turn(lastDeg)
        resList.append(self.colorChecker.color())
 
        #3.检查测量的颜色是不是想要的，resIndex是最终检测到的下标
        # print("checkObjectByLargeDegree---start-2--")
        print(resList)
        checkInd=0
        reCheck=[]
        resIndex=0
        ind=0
        while checkInd< len(resList):
            if (str(resList[checkInd])==str(checkColor)):
                reCheck.append(checkInd)
            checkInd=checkInd+1
        print("reCheck:")
        print(reCheck)
        if len(reCheck)>1:
              temp=int(len(reCheck)/2)
              resIndex = reCheck[temp]
        else:
              temp=int( len(resList)/2)
              resIndex=temp

        print("resIndex "+ str(resIndex))  
        #4.根据下标，调整到对应的角度
        if(resIndex < i):
            lastTurnDeg=(lastDeg+ (i-resIndex-1)*stepDegree - offset)*-1 
            print("turn to the resIndex degree "+ str(lastTurnDeg))
            self.turn(lastTurnDeg)
        else:
            print("No need turn since current is resIndex")
         
        #5.返回找不找到
        if len(reCheck)>1:
            return True
        else:
            return False

    def goTask1_Group1(self):
        print("1.初始化,抬起抓杆")
        self.paySound(['C4/4'])
        # self.zhuaGanUpByCheck(180*25)
        print("#2.后退")
        self.goAhead(-290)
        print("#3.转弯")
        self.turn(19*2)
        print("#4.后退")
        self.goAhead(-475)
        print("#5.转弯 面向杯子")
        self.turn(19*2+5)
        # print("#6.调整角度")
        # self.checkObjectByLargeDegree(-4,4,4,0)
        print("#7.前进，找第一个硬币")
        self.goAhead(180)
        print("#8. 放下抓杆")
        self.zhuaGanDown(180*15 + 120 )

    def goTask1_Group2(self):
        print("#9.检查是否是红色")
        self.paySound(['D4/4'])
        redPosition = 0
        bluePosition = 0
        
        foundRed = False
        goCheckCount = 0
        count = 0
        accumulatedLenght = 0

        while (goCheckCount < 2 and not(foundRed)):
            goCheckCount = goCheckCount + 1
            while (count < 1) and not(foundRed):
                if (self.checkColorByLargeDegree(Color.RED,-5*(7-goCheckCount),5*(7-goCheckCount),5,-1)):
                    print( "found Red")
                    foundRed = True
                    redPosition = 1
                    bluePosition = 2
                    break
                count = count + 1
            self.goAhead(35)
            accumulatedLenght = accumulatedLenght +35
            print (" 1 accumulatedLenght :"+ str(accumulatedLenght))
            count=0

       
        if (foundRed and redPosition==1) :
                print("#10.A 找到红色在位置1上，抓取")
                self.goAhead(-180)
                self.zhuaGanDown(180*2)
                self.zhuaZiClose(180*12)
                self.zhuaGanUpByCheck(180*25)
                self.checkObjectByLargeDegree(-4,4,4,0)
                self.goAhead(380)
                
        else:          
          print("#10.B 去在位置2上找红色")
          self.goAhead(165 - accumulatedLenght)

          goCheckCount = 0
          count = 0
          while (goCheckCount < 2 and not(foundRed)):
                goCheckCount = goCheckCount + 1
                while (count < 1) and not(foundRed):
                    if (self.checkColorByLargeDegree(Color.RED,-3*(7-goCheckCount),3*(7-goCheckCount),3,-1)):
                        print("#10.B 在位置2上找到了红色")
                        foundRed = True
                        redPosition = 2
                        bluePosition = 1
                        break
                    count = count + 1
                # self.goAhead(30)
                count=0
        
          print("#10.B 在位置2上找到了红色,抓取")
          if (foundRed and redPosition==2) :
                self.goAhead(-180)
                self.zhuaGanDown(180*2)
                self.zhuaZiClose(180*12)
                self.zhuaGanUpByCheck(180*25)
                self.checkObjectByLargeDegree(-4,4,4,0)
                self.goAhead(380)
                
                self.ev3.speaker.say("第一任务完成")
          else:
            print("#ERROR 出错了。任务失败")

    def goTask1_Group3(self):
        self.paySound(['E4/4'])
        print("#11 前往水杯")
        cupDis=self.checkObjectByLargeDegree(-15,15,5)
        cupDis=self.checkObjectByLargeDegree(-10,10,8)
        while cupDis>=200:
           goDis= (cupDis - 200)
           if goDis > 30:
             self.goAhead(goDis/2)
           else:
             self.goAhead(goDis)
           cupDis=self.checkObjectByLargeDegree(-5,5,5)
        
        print("#12 放下红色到水杯")  
        self.zhuaZiOpen(180*12)


    def goTask1_Group4(self):
        self.paySound(['F4/4'])
        print("#13 后退，找到蓝色")  
          
        print("#14 后退，找到蓝色，抓取")  

        print("#15 回到中心点")  

        print("#16 前往任务二")  

        print("#17 接近任务二")  

        print("#18 放下蓝色到水杯")  



    def goTask1(self):
        print("GoTask1 start---")   
        self.ev3.speaker.say("第一任务开始")     
        # self.goTask1_Group1()
        self.goTask1_Group2()
        # self.goTask1_Group3()
        # self.goTask1_Group4()           
        print("GoTask1 Finish---")
